{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Notebook Setup \n",
    "The following cell will install Drake, checkout the underactuated repository, and set up the path (only if necessary).\n",
    "- On Google's Colaboratory, this **will take approximately two minutes** on the first time it runs (to provision the machine), but should only need to reinstall once every 12 hours.  Colab will ask you to \"Reset all runtimes\"; say no to save yourself the reinstall.\n",
    "- On Binder, the machines should already be provisioned by the time you can run this; it should return (almost) instantly.\n",
    "\n",
    "More details are available [here](http://underactuated.mit.edu/drake.html)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "try:\n",
    "  import pydrake\n",
    "  import underactuated\n",
    "except ImportError:\n",
    "  !curl -s https://raw.githubusercontent.com/RussTedrake/underactuated/master/scripts/setup/jupyter_setup.py > jupyter_setup.py\n",
    "  from jupyter_setup import setup_underactuated\n",
    "  setup_underactuated()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import copy\n",
    "import numpy as np\n",
    "\n",
    "from pydrake.common.containers import namedview\n",
    "from pydrake.systems.framework import (BasicVector, LeafSystem, PublishEvent,\n",
    "                                       PortDataType, UnrestrictedUpdateEvent,\n",
    "                                       WitnessFunctionDirection)\n",
    "from pydrake.systems.pyplot_visualizer import PyPlotVisualizer\n",
    "\n",
    "\n",
    "SLIPState = namedview(\n",
    "    \"SLIPState\", [\"x\", \"z\", \"r\", \"theta\", \"xdot\", \"zdot\", \"rdot\", \"thetadot\"])\n",
    "\n",
    "\n",
    "class SpringLoadedInvertedPendulum(LeafSystem):\n",
    "\n",
    "    def __init__(self):\n",
    "        LeafSystem.__init__(self)\n",
    "\n",
    "        self.DeclareVectorInputPort(\"touchdown_angle\", BasicVector(1))\n",
    "        self.DeclareContinuousState(BasicVector(np.zeros(8)), 4, 4, 0)\n",
    "\n",
    "        self.DeclareVectorOutputPort(\"state\", BasicVector(8), self.CopyStateOut)\n",
    "\n",
    "        # Parameters from Geyer05, p.23\n",
    "        self.mass = 80.  # kg\n",
    "        self.r0 = 1.  # m\n",
    "        self.gravity = 9.81  # m/s^2\n",
    "        # Define spring constant in terms of the dimensionless number.\n",
    "        # Definition in section 2.4.3, values in figure 2.4.\n",
    "        # Note: Geyer05 says 10.8 (which doesn't work? -- I get no fixed pts).\n",
    "        dimensionless_spring_constant = 10.7\n",
    "        self.stiffness = (dimensionless_spring_constant * self.mass *\n",
    "                          self.gravity / self.r0)\n",
    "\n",
    "        self.last_apex = None  # placeholder for writing return map result.\n",
    "\n",
    "        self.touchdown_witness = self.MakeWitnessFunction(\n",
    "            \"touchdown\", WitnessFunctionDirection.kPositiveThenNonPositive,\n",
    "            self.foot_height, UnrestrictedUpdateEvent(self.touchdown))\n",
    "        self.takeoff_witness = self.MakeWitnessFunction(\n",
    "            \"takeoff\", WitnessFunctionDirection.kPositiveThenNonPositive,\n",
    "            self.leg_compression, UnrestrictedUpdateEvent(self.takeoff))\n",
    "        self.apex_witness = self.MakeWitnessFunction(\n",
    "            \"apex\", WitnessFunctionDirection.kPositiveThenNonPositive,\n",
    "            self.apex, PublishEvent(self.publish_apex))\n",
    "\n",
    "    def foot_height(self, context):\n",
    "        s = SLIPState(context.get_continuous_state_vector().CopyToVector())\n",
    "        return s.z - self.r0 * np.cos(s.theta)\n",
    "\n",
    "    def touchdown(self, context, event, state):\n",
    "        s = SLIPState(\n",
    "            context.get_mutable_continuous_state_vector().CopyToVector())\n",
    "\n",
    "        # print(\"touchdown\")\n",
    "\n",
    "        # Update rdot and thetadot to match xdot and ydot, using\n",
    "        # x = -r*sin(theta), z = r*cos(theta)\n",
    "        #  => xdot = -rdot*s - r*c*thetadot, zdot = rdot*c - r*s*thetadot\n",
    "        #  => xdot*c + zdot*s = -r*thetadot\n",
    "        # r^2 = x^2 + z^2\n",
    "        #  => 2r*rdot = 2x*xdot + 2z*zdot\n",
    "        #  => rdot = -xdot*sin(theta) + zdot*cos(theta)\n",
    "        # (matches Geyer05 Eq. 2.24 up to the symbol changes)\n",
    "        s.r = self.r0\n",
    "        s.rdot = -s.xdot * np.sin(s.theta) + s.zdot * np.cos(s.theta)\n",
    "        s.thetadot = -(s.xdot * np.cos(s.theta) +\n",
    "                       s.zdot * np.sin(s.theta)) / self.r0\n",
    "        state.get_mutable_continuous_state().get_mutable_vector().SetFromVector(\n",
    "            s[:])\n",
    "\n",
    "    def leg_compression(self, context):\n",
    "        s = SLIPState(context.get_continuous_state_vector().CopyToVector())\n",
    "        return self.r0 - s.r\n",
    "\n",
    "    def takeoff(self, context, event, state):\n",
    "        s = SLIPState(\n",
    "            context.get_mutable_continuous_state_vector().CopyToVector())\n",
    "\n",
    "        # print(\"takeoff\")\n",
    "\n",
    "        # Setup flight state (these lines aren't strictly required, since we\n",
    "        # choose to also integrate x and z in stance below).\n",
    "        s.z = self.r0 * np.cos(s.theta)\n",
    "        s.xdot = -s.rdot * np.sin(s.theta) - self.r0 * s.thetadot * np.cos(\n",
    "            s.theta)\n",
    "        s.zdot = s.rdot * np.cos(s.theta) - self.r0 * s.thetadot * np.sin(\n",
    "            s.theta)\n",
    "\n",
    "        # Update theta to commanded leg angle.\n",
    "        s.theta = self.EvalVectorInput(context, 0).GetAtIndex(0)\n",
    "        s.thetadot = 0\n",
    "        s.r = self.r0\n",
    "        s.rdot = 0\n",
    "\n",
    "        state.get_mutable_continuous_state().get_mutable_vector().SetFromVector(\n",
    "            s[:])\n",
    "\n",
    "    def apex(self, context):\n",
    "        return context.get_continuous_state_vector().GetAtIndex(5)  # zdot\n",
    "\n",
    "    def publish_apex(self, context, event):\n",
    "        # TODO(russt): provide an option to terminate here instead, pending\n",
    "        # resolution of #4447.\n",
    "        # print(\"apex\")\n",
    "        if self.last_apex is None:\n",
    "            s = SLIPState(\n",
    "                context.get_mutable_continuous_state_vector().CopyToVector())\n",
    "            self.last_apex = s.z\n",
    "\n",
    "    def apex_velocity_from_dimensionless_system_energy(self, Etilde, z):\n",
    "        E = Etilde * self.mass * self.gravity * self.r0\n",
    "        # E = 0.5*m*v^2 + m*g*z\n",
    "        xdot = np.sqrt(2. / self.mass * (E - self.mass * self.gravity * z))\n",
    "        return xdot\n",
    "\n",
    "    def energy_flight(self, context):\n",
    "        s = SLIPState(\n",
    "            context.get_mutable_continuous_state_vector().CopyToVector())\n",
    "        return (0.5 * self.mass * (s.xdot**2 + s.zdot**2) +\n",
    "                self.mass * self.gravity * s.z)\n",
    "\n",
    "    def energy_stance(self, context):\n",
    "        s = SLIPState(\n",
    "            context.get_mutable_continuous_state_vector().CopyToVector())\n",
    "        return (0.5 * self.mass * (s.rdot**2 + s.r**2 * s.thetadot**2) +\n",
    "                self.mass * self.gravity * s.r * np.cos(s.theta) +\n",
    "                0.5 * self.stiffness * (self.r0 - s.r)**2)\n",
    "\n",
    "    def CopyStateOut(self, context, output):\n",
    "        x = context.get_continuous_state_vector().CopyToVector()\n",
    "        y = output.SetFromVector(x)\n",
    "\n",
    "    def DoGetWitnessFunctions(self, context):\n",
    "        return [self.touchdown_witness, self.takeoff_witness, self.apex_witness]\n",
    "\n",
    "    def DoCalcTimeDerivatives(self, context, derivatives):\n",
    "        s = SLIPState(context.get_continuous_state_vector().CopyToVector())\n",
    "        sdot = SLIPState(np.zeros(8))\n",
    "        sdot[0:4] = s[4:8]\n",
    "\n",
    "        if (self.foot_height(context) < 0):\n",
    "            # then we're in \"stance\"\n",
    "            sdot.rdot = (self.stiffness / self.mass * (self.r0 - s.r) +\n",
    "                         s.r * s.thetadot**2 - self.gravity * np.cos(s.theta))\n",
    "            sdot.thetadot = (self.gravity / s.r * np.sin(s.theta) -\n",
    "                             2 * s.rdot * s.thetadot / s.r)\n",
    "\n",
    "            # Integrate x and z also, just for the sake of visualization (all\n",
    "            # the integrated values except x will be overwritten in the\n",
    "            # take-off reset).\n",
    "            # x = -r*sin(theta), y = r*cos(theta) =>\n",
    "            sdot.xdot = (-sdot.rdot * np.sin(s.theta) -\n",
    "                         2 * s.rdot * s.thetadot * np.cos(s.theta) +\n",
    "                         s.r * s.thetadot**2 * np.sin(s.theta) -\n",
    "                         s.r * sdot.thetadot * np.cos(s.theta))\n",
    "            sdot.zdot = (sdot.rdot * np.cos(s.theta) -\n",
    "                         2 * s.rdot * s.thetadot * np.sin(s.theta) -\n",
    "                         s.r * sdot.thetadot * np.sin(s.theta) -\n",
    "                         s.r * s.thetadot**2 * np.cos(s.theta))\n",
    "\n",
    "        else:\n",
    "            sdot.xdot = 0\n",
    "            sdot.zdot = -self.gravity\n",
    "            sdot.rdot = 0\n",
    "            sdot.thetadot = 0\n",
    "\n",
    "        derivatives.get_mutable_vector().SetFromVector(sdot[:])\n",
    "        \n",
    "\n",
    "class SLIPVisualizer(PyPlotVisualizer):\n",
    "\n",
    "    def __init__(self, ax=None, show=True):\n",
    "        PyPlotVisualizer.__init__(self, ax=ax, show=show)\n",
    "        self.DeclareInputPort(PortDataType.kVectorValued, 8)\n",
    "        self.ax.set_aspect(\"equal\")\n",
    "        self.ax.set_xlim(0, 2)\n",
    "        self.ax.set_ylim(-2, 2)\n",
    "\n",
    "        # Draw the ground.\n",
    "        self.ax.plot([-50, 50], [0, 0], \"k\")\n",
    "\n",
    "        a = np.linspace(0, 2 * np.pi, 50)\n",
    "        radius = 0.1\n",
    "        self.hip_fill = self.ax.fill(radius * np.sin(a),\n",
    "                                     radius * np.cos(a),\n",
    "                                     zorder=1,\n",
    "                                     edgecolor=\"k\",\n",
    "                                     facecolor=[.6, .6, .6])\n",
    "        self.hip = copy.copy(self.hip_fill[0].get_path().vertices)\n",
    "\n",
    "        self.leg_line = [self.ax.plot([0, 0], [0, -1], \"k\")[0]]\n",
    "        self.leg_data = [self.leg_line[0].get_xydata().T]\n",
    "        for i in range(1, 13):\n",
    "            self.leg_line.append(\n",
    "                self.ax.plot(\n",
    "                    0.1 * np.array(\n",
    "                        [np.sin((i - 1) * np.pi / 2.),\n",
    "                         np.sin(i * np.pi / 2.)]),\n",
    "                    -.2 - .7 / 13 * np.array([i - 1, i]), \"k\")[0])\n",
    "            self.leg_data.append(self.leg_line[i].get_xydata().T)\n",
    "\n",
    "    def draw(self, context):\n",
    "        state = SLIPState(self.EvalVectorInput(context, 0).CopyToVector())\n",
    "\n",
    "        self.hip_fill[0].get_path().vertices[:, 0] = state.x + self.hip[:, 0]\n",
    "        self.hip_fill[0].get_path().vertices[:, 1] = state.z + self.hip[:, 1]\n",
    "\n",
    "        R = np.array([[np.cos(state.theta), -np.sin(state.theta)],\n",
    "                      [np.sin(state.theta),\n",
    "                       np.cos(state.theta)]])\n",
    "        for i in range(len(self.leg_line)):\n",
    "            self.leg_line[i].set_xdata(state.x +\n",
    "                                       state.r * R[0, :].dot(self.leg_data[i]))\n",
    "            self.leg_line[i].set_ydata(state.z +\n",
    "                                       state.r * R[1, :].dot(self.leg_data[i]))\n",
    "\n",
    "        self.ax.set_title(\"t = {:.1f}\".format(context.get_time()))\n",
    "\n",
    "        \n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "from IPython import get_ipython\n",
    "from IPython.display import HTML\n",
    "from underactuated.jupyter import AdvanceToAndVisualize, SetupMatplotlibBackend\n",
    "plt_is_interactive = SetupMatplotlibBackend()\n",
    "\n",
    "from pydrake.systems.analysis import Simulator\n",
    "from pydrake.systems.framework import DiagramBuilder\n",
    "from pydrake.systems.primitives import ConstantVectorSource, LogOutput\n",
    "\n",
    "builder = DiagramBuilder()\n",
    "plant = builder.AddSystem(SpringLoadedInvertedPendulum())\n",
    "\n",
    "# Parameters from Geyer05, Figure 2.4\n",
    "# Note: Geyer uses angle of attack = 90 - touchdown_angle\n",
    "touchdown_angle = np.deg2rad(30)\n",
    "Etilde = 1.61\n",
    "\n",
    "s = SLIPState(np.zeros(8))\n",
    "s.z = 0.9\n",
    "s.theta = touchdown_angle\n",
    "s.r = 1\n",
    "s.xdot = plant.apex_velocity_from_dimensionless_system_energy(Etilde, s.z)\n",
    "\n",
    "visualizer = builder.AddSystem(SLIPVisualizer(show=plt_is_interactive))\n",
    "builder.Connect(plant.get_output_port(0), visualizer.get_input_port(0))\n",
    "\n",
    "log = LogOutput(plant.get_output_port(0), builder)\n",
    "\n",
    "command = builder.AddSystem(ConstantVectorSource([touchdown_angle]))\n",
    "builder.Connect(command.get_output_port(0), plant.get_input_port(0))\n",
    "\n",
    "diagram = builder.Build()\n",
    "simulator = Simulator(diagram)\n",
    "context = simulator.get_mutable_context()\n",
    "context.SetAccuracy(1e-10)\n",
    "\n",
    "context.get_mutable_continuous_state_vector().SetFromVector(s[:])\n",
    "\n",
    "duration = 2.0 if get_ipython() else 0.1 # sets a shorter duration during testing\n",
    "AdvanceToAndVisualize(simulator, visualizer, duration)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Apex-to-apex map"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "plant = SpringLoadedInvertedPendulum()\n",
    "\n",
    "# Parameters from Geyer05, Figure 2.4\n",
    "# Note: Geyer uses angle of attack = 90 - touchdown_angle\n",
    "touchdown_angle = np.deg2rad(30)\n",
    "Etilde = 1.61\n",
    "\n",
    "s = SLIPState(np.zeros(8))\n",
    "s.theta = touchdown_angle\n",
    "s.r = 1\n",
    "\n",
    "simulator = Simulator(plant)\n",
    "context = simulator.get_mutable_context()\n",
    "context.FixInputPort(0, [touchdown_angle])\n",
    "context.SetAccuracy(1e-5)\n",
    "\n",
    "zs = np.linspace(np.cos(touchdown_angle) + 0.001, 0.95, 25)\n",
    "zns = 0 * zs\n",
    "for i in range(len(zs)):\n",
    "    s.z = zs[i]\n",
    "    s.xdot = plant.apex_velocity_from_dimensionless_system_energy(Etilde, s.z)\n",
    "    context.SetTime(0.)\n",
    "    context.get_mutable_continuous_state_vector().SetFromVector(s[:])\n",
    "    simulator.Initialize()\n",
    "    # Note: With this duration, I sometimes get an extra \"touchdown\" after the\n",
    "    # apex, which results in apex-touchdown; touchdown-takeoff-apex on the\n",
    "    # console.  It's not a double reset, the consecutive touchdowns are two\n",
    "    # different sims.\n",
    "    simulator.AdvanceTo(0.6)\n",
    "    zns[i] = plant.last_apex\n",
    "    plant.last_apex = None\n",
    "\n",
    "fix, ax = plt.subplots()\n",
    "ax.plot(zs, zns)\n",
    "ax.plot(zs, zs)\n",
    "ax.axis(\"equal\")\n",
    "ax.set_xlim([zs[0], zs[-1]])\n",
    "ax.set_ylim([zs[0], zs[-1]])\n",
    "ax.set_xlabel(\"apex height z[n]\")\n",
    "ax.set_ylabel(\"apex height z[n+1]\")\n",
    "\n",
    "plt.show()\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.7"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
